package com.linlich.mahonyfilter;

import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.os.Bundle;
import android.support.v7.app.AppCompatActivity;
import android.widget.TextView;

import static java.lang.Math.asin;
import static java.lang.Math.atan2;

public class MainActivity extends AppCompatActivity {
    private SensorManager mSensorManager;
    private TextView mah_View, ori_View;
    private static final float NS2S = 1.0f / 1000000000.0f;
    private float timestamp;
//    float Kp = 3.5f, Ki = 0.05f;
    float Kp = 2.0f, Ki = 0.005f;   //论文提供
    float exInt, eyInt, ezInt;
    float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f ,q3 = 0.0f;

    @Override
    protected void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
        setContentView(R.layout.activity_main);
        mah_View = (TextView) findViewById(R.id.mahony_Filter);
        ori_View = (TextView) findViewById(R.id.oritation);

        mSensorManager = (SensorManager) getSystemService(Context.SENSOR_SERVICE);

        Sensor accSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
        Sensor gyrSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);

        mSensorManager.registerListener(listener, accSensor, mSensorManager.SENSOR_DELAY_NORMAL);
        mSensorManager.registerListener(listener, gyrSensor, mSensorManager.SENSOR_DELAY_NORMAL);
    }

    @Override
    protected void onDestroy() {
        super.onDestroy();
        if (mSensorManager != null) {
            mSensorManager.unregisterListener(listener);
        }
    }

    private SensorEventListener listener = new SensorEventListener() {
        float[] accValues = new float[3];
        float[] gyrValues = new float[3];
        float[] angle = new float[3];

        @Override
        public void onSensorChanged(SensorEvent event) {
            if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) {
                accValues = event.values.clone();
            }else if (event.sensor.getType() == Sensor.TYPE_GYROSCOPE) {
                gyrValues = event.values.clone();
            }

            if (timestamp != 0) {
                final float dT = (event.timestamp - timestamp) * NS2S;
                IMUupdate(gyrValues, accValues, dT / 2);
                angle[0] += gyrValues[0] * dT;
                angle[1] += gyrValues[1] * dT;
                angle[2] += gyrValues[2] * dT;
                ori_View.setText("pitch: " + angle[0] * 57.3 + "\n" +
                        "roll: " + angle[1] * 57.3 + "\n" +
                        "azimuth: " + angle[2] * 57.3);
            }
            timestamp = event.timestamp;

        }

        @Override
        public void onAccuracyChanged(Sensor sensor, int accuracy) {

        }
    };

    public void IMUupdate(float[] gyr, float[] acc, float halfT) {
        float norm;
//        float hx, hy, hz, bx, bz, wx, wy, wz;
        float vx, vy, vz;
        float ex, ey, ez;
        double[] values = new double[3];//方位角 roll pitch yaw

        float q0q0 = q0 * q0;
        float q0q1 = q0 * q1;
        float q0q2 = q0 * q2;
//        float q0q3 = q0 * q3;
        float q1q1 = q1 * q1;
//        float q1q2 = q1 * q2;
        float q1q3 = q1 * q3;
        float q2q2 = q2 * q2;
        float q2q3 = q2 * q3;
        float q3q3 = q3 * q3;

        if (acc[0] * acc[1] * acc[2] == 0) {
            return;
        }

        // 第一步：
        // 对加速度数据进行归一化
        norm = (float) Math.sqrt(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]);
        acc[0] = acc[0] / norm;
        acc[1] = acc[1] / norm;
        acc[2] = acc[2] / norm;

//        // 对磁力数据进行归一化
//        norm = (float) Math.sqrt(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
//        mag[0] = mag[0] / norm;
//        mag[1] = mag[1] / norm;
//        mag[2] = mag[2] / norm;
//
//        // 计算磁力参考方向
//        hx = (float) (2 * mag[0] * (0.5 - q2q2 - q3q3) + 2 * mag[1] * (q1q2 - q0q3) + 2 * mag[2] * (q1q3 + q0q2));
//        hy = (float) (2 * mag[0] * (q1q2 + q0q3) + 2 * mag[1] * (0.5 - q1q1 - q3q3) + 2 * mag[2] * (q2q3 - q0q1));
//        hz = (float) (2 * mag[0] * (q1q3 - q0q2) + 2 * mag[1] * (q2q3 + q0q1) + 2 * mag[2] * (0.5 - q1q1 - q2q2));
//        bx = (float) Math.sqrt((hx*hx) + (hy*hy));
//        bz = hz;

        // 第二步：DCM矩阵旋转
        vx = 2 * (q1q3 - q0q2);
        vy = 2 * (q0q1 + q2q3);
        vz = q0q0 - q1q1 - q2q2 + q3q3;

//        wx = (float) (2 * bx * (0.5 - q2q2 - q3q3) + 2 * bz * (q1q3 - q0q2));
//        wy = 2 * bx * (q1q2 - q0q3) + 2 * bz * (q0q1 + q2q3);
//        wz = (float) (2 * bx * (q0q2 + q1q3) + 2 * bz * (0.5 - q1q1 - q2q2));

        // 第三步：在机体坐标系下做向量叉积得到补偿数据
        ex = acc[1] * vz - acc[2] * vy;
        ey = acc[2] * vx - acc[0] * vz;
        ez = acc[0] * vy - acc[1] * vx;

//        ex = (acc[1] * vz - acc[2] * vy) + (mag[1] * wz - mag[2] * wy);
//        ey = (acc[2] * vx - acc[0] * vz) + (mag[2] * wx - mag[0] * wz);
//        ez = (acc[0] * vy - acc[1] * vx) + (mag[0] * wy - mag[1] * wx);

        // 第四步：对误差进行PI计算，补偿角速度
        exInt = exInt + ex * Ki;
        eyInt = eyInt + ey * Ki;
        ezInt = ezInt + ez * Ki;

        gyr[0] = gyr[0] + Kp * ex + exInt;
        gyr[1] = gyr[1] + Kp * ey + eyInt;
        gyr[2] = gyr[2] + Kp * ez + ezInt;

        // 第五步：按照四元数微分公式进行四元数更新
        q0 = q0 + (-q1 * gyr[0] - q2 * gyr[1] - q3 * gyr[2]) * halfT;
        q1 = q1 + (q0 * gyr[0] + q2 * gyr[2] - q3 * gyr[1]) * halfT;
        q2 = q2 + (q0 * gyr[1] - q1 * gyr[2] + q3 * gyr[0]) * halfT;
        q3 = q3 + (q0 * gyr[2] + q1 * gyr[1] - q2 * gyr[0]) * halfT;

        norm = (float) Math.sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
        q0 = q0/norm;
        q1 = q1/norm;
        q2 = q2/norm;
        q3 = q3/norm;

        values[0] = -atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 57.3;
        values[1] = asin(2 * q1 * q3 - 2 * q0 * q2) * 57.3;
        values[2] = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3;

        mah_View.setText(/*"航向角： " + values[0] + "\n" +*/
                "pitch: " + values[2] + "\n" +
                "roll: " + values[1]);
    }
}
